#!/usr/bin/env python

import roslib; roslib.load_manifest('team4')
import rospy
from lift_test import Tray 
from geometry_msgs.msg import PointStamped
from ObjectUnstacker import *
from ObjectStacker import *
from PlatePicker import *
from Room3 import *
from free_table_spot import *
from pr2_python import base, head, gripper, world_interface
#from filename_of_your_module import * <- does not need the .py. Should import classes, functions, and maybe global vars

# Before calling import on your file, make sure of three things:
   # if it is team4/ then it can stay in scripts/ or be in src/
   # if it is in stacked_objects/ it has to be in src/
   # there cannot be an init_node in your code. If your code needs that to run, then you need an interface of some sort and we need to launch it seperately

# If there is anything that your code needs launched in addition to the usual, add it to the demo.launch file.

# Really, this should be done as a state machine. Just pretend it's one for now.


def part2():
    tray = Tray()
    base_mover = base.Base()
    stacker = ObjectStacker()
    picker = PlatePicker()
    room3 = Room3()
    ou = ObjectUnstacker()

    gr_r = gripper.Gripper('right_arm')
    gr_r.open()
    gr_l = gripper.Gripper('left_arm')
    gr_l.open()

    print "Press key to move to stacking area"
    raw_input()

    rospy.loginfo("Stacking items on tray")
    # stacking objects code
    print "Press key to stack objects"
    raw_input()
    stacker.stack_objects()
    #or
    #stacker.pose_stack_objects(p)
    
    stacker.move_arms_back()
   
    # PART 2 DONE
     

if __name__ == "__main__":
   rospy.init_node("team4_demo")
   
   rospy.loginfo("Beginning team 4 demo")
   rospy.loginfo("Creating all class instances")
   # instantiate classes here (if they need instantiating)
   
   #part1()
   part2()
   #part3()
   #part4()
   

